import cv2
import time
import os
import MazeTreasureHunt
import traceback
import serial
import update_direction


def CalcDir(dict):
    out_dict = {}
    keys = dict.keys()
    for key in keys:
        result = []
        for path in dict[key]:
            out = update_direction.CalcDir(path)
            result.append(out)
        result[0] = result[0][1:]
        result = MazeTreasureHunt.MazeTreasureHunt.clean_lists(result)
        out_dict[key + 'Dir'] = result
    return out_dict


def MazeProcess(mazeHunt, image):
    try:
        start_time = time.time()
        mazeHunt.LoadImgResource(image)
        paths = mazeHunt.Process()
    except Exception as e:
        print("Get Error: " + str(e))
        traceback.print_exc()
        return None
    else:
        end_time = time.time()
        print("Process Run Time: ", end_time - start_time)
        print(paths)
        result = CalcDir(paths)
        print(result)
        return result


if __name__ == "__main__":
    ser = None
    result = None
    mazeHunt = MazeTreasureHunt.MazeTreasureHunt()
    mazeHunt.SetPlotUtilsEnable(False)  # 关闭绘图
    mazeHunt.SetDebugEnable(True)  # 开启debug imshow
    # 检查环境变量"RASPBERRY_DEVICE"的值
    raspberry_device = os.getenv("RASPBERRY_DEVICE", "").lower() == "true"

    if raspberry_device:
        from picamera import PiCamera
        from picamera.array import PiRGBArray

        ser = serial.Serial('/dev/ttyAMA0', 115200)
        camera = PiCamera()
        camera.resolution = (640, 480)
        camera.framerate = 32
        camera.hflip = True
        camera.vflip = True
        rawCapture = PiRGBArray(camera, size=(640, 480))
        # allow the camera to warmup
        time.sleep(0.1)
        # capture frames from the camera
        for frame in camera.capture_continuous(rawCapture, format="bgr", use_video_port=True):
            image = frame.array
            cv2.imshow("preview src image", image)
            result = MazeProcess(mazeHunt, image)
            if result:
                break

            cv2.waitKey(1)
            rawCapture.truncate(0)
        camera.close()
    else:
        ser = serial.Serial('/dev/tty.wchusbserial1420', 115200)
        # 直接使用opencv获取摄像头的视频流并显示到屏幕上
        cap = cv2.VideoCapture(0)
        while True:
            ret, frame = cap.read()
            frame = cv2.imread('./2023-07-25_22-58-34.jpg')
            cv2.imshow("preview src image", frame)
            result = MazeProcess(mazeHunt, frame)
            if result:
                break

            cv2.waitKey(1)
        cap.release()
    cv2.destroyAllWindows()
    if not ser.isOpen:
        ser.open()
    ser.flushInput()

    # 原来命令数组索引为0-8
    # 每个宝藏点到出口数组索引为new_command：9-15（除去第八个宝藏点）
    # 起点到每个宝藏的路径reset_index：16-22
    cnt = 0
    true_treasure = 0
    A_count = 0
    while True:
        recv = ser.inWaiting()
        if recv:
            recv_byte = ser.read()
            if recv_byte == b'A':  # 32启动时树莓派接收到的A
                A_count += 1
                if A_count == 1:
                    ser.write(result['CommonPathDir'][cnt])
                elif A_count == 2:  # 重启一次，写死，最多重启一下，寄了就寄了
                    ser.write(result['StartPathDir'][cnt])
                A_count = 0
            elif recv_byte == b'S':  # 每启动一下宝藏识别索引后移一位，写死，错误启动可直接跳过某一点
                treasure_color = mazeHunt.GetTrea()
                ser.write(treasure_color)
                cnt += 1
            elif recv_byte == b'1':
                true_treasure += 1
                if true_treasure == 3:  # 三个真宝藏
                    ser.write(result['EndPathDir'][cnt])
                    break
            elif recv_byte == b'C':  # 识别宝藏之后请求的路径
                ser.write(result['CommonPathDir'][cnt])
            recv_byte = 0
